#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <Eigen/Core>
#include <sophus/se3.hpp>

using namespace cv;
using namespace std;

void findmatch(const Mat &image1, const Mat &image2, vector<KeyPoint> &keypoints1, 
               vector<KeyPoint> &keypoints2, vector<DMatch> &goodmatches)
{
    Mat descriptors1,descriptors2;
    Ptr<FeatureDetector> detector=ORB::create();
    Ptr<DescriptorExtractor> descriptor=ORB::create();
    Ptr<DescriptorMatcher> matcher=DescriptorMatcher::create("BruteForce-Hamming");
    
    detector->detect(image1, keypoints1);
    detector->detect(image2, keypoints2);
    
    descriptor->compute(image1,keypoints1,descriptors1);
    descriptor->compute(image2,keypoints2,descriptors2);
    
    vector<DMatch> matches;
    matcher->match(descriptors1,descriptors2,matches);
    
    auto min_max=minmax_element(matches.begin(), matches.end(),[](DMatch &m1, DMatch &m2){
        return m1.distance>m2.distance;
    });
    
    double max_dist=min_max.first->distance;
    double min_dist=min_max.second->distance;
    
    for(int i=0; i<matches.size();i++){
        if(matches[i].distance<max(2*min_dist,30.0)){
            goodmatches.push_back(matches[i]);
        }
    }
}

void getmatchpoint(const vector<KeyPoint> &keypoints1, const vector<KeyPoint> &keypoints2, 
                   const vector<DMatch> &goodmatches, vector<Point2f> &arraypoint1, vector<Point2f> &arraypoint2)
{
    for(int i=0; i<goodmatches.size();i++){
        arraypoint1.push_back(keypoints1[goodmatches[i].queryIdx].pt);
        arraypoint2.push_back(keypoints2[goodmatches[i].trainIdx].pt);
    }
}

void getcameraK(const double &focal_length, const double &alph, 
                const double &imagewidth, const double &imagehight,
               Mat &K) 
{
    K=(Mat_<double>(3,3)<<focal_length, 0, imagewidth/2, 0, alph*focal_length, imagehight/2, 0,0,1);
}

void gettransmat(const vector<Point2f> &points1, const vector<Point2f> &points2, 
                 const Mat &K,
                 Mat &Fun, Mat &Ess, 
                 Mat &R10, Mat &t10, Mat &R20, Mat &t20, Mat &R21, Mat &t21)
{
    Fun=findFundamentalMat(points1, points2, CV_FM_8POINT);
    
    double focal_length=K.at<double>(0,0);
    Point2f principalpoint;
    principalpoint.x=K.at<double>(0,2);
    principalpoint.y=K.at<double>(1,2);
    Ess=findEssentialMat(points1, points2, focal_length, principalpoint);
    recoverPose(Ess, points1, points2, R21, t21, focal_length, principalpoint);
    
    Mat post10=Mat::eye(Size(4,4),CV_64F);
                                        
    Mat post21=(Mat_<double>(4,4)<<R21.at<double>(0,0), R21.at<double>(0,1), R21.at<double>(0,2), t21.at<double>(0,0),
                                    R21.at<double>(1,0), R21.at<double>(1,1), R21.at<double>(1,2), t21.at<double>(1,0),
                                    R21.at<double>(2,0), R21.at<double>(2,1), R21.at<double>(2,2), t21.at<double>(2,0),
                                    0, 0, 0, 1);
     
     Mat post20=post21*post10;
     
     R20=(Mat_<double>(3,3)<<post20.at<double>(0,0), post20.at<double>(0,1), post20.at<double>(0,2),
                            post20.at<double>(1,0), post20.at<double>(1,1), post20.at<double>(1,2),
                            post20.at<double>(2,0), post20.at<double>(2,1), post20.at<double>(2,2));
     
     t20=(Mat_<double>(3,1)<<post20.at<double>(0,3), post20.at<double>(1,3), post20.at<double>(2,3));

}

void piex2rayplane(const vector<Point2f> &ppoints, const Mat &K, vector<Point2f> &rpoints)
 {
     
     Mat invK;
     invert(K,invK, DECOMP_LU);
     
     Mat raypoint;
     Mat piexpoint;
     Point2f rpoint;
     
     for(int i=0; i<ppoints.size( ); i++){
     piexpoint=(Mat_<double>(3,1)<<ppoints[i].x, ppoints[i].y, 1);
     double a=invK.at<double>(0,0)*piexpoint.at<double>(0,0)+
              invK.at<double>(0,1)*piexpoint.at<double>(1,0)+
              invK.at<double>(0,2)*piexpoint.at<double>(2,0);
     double b=invK.at<double>(1,0)*piexpoint.at<double>(0,0)+
              invK.at<double>(1,1)*piexpoint.at<double>(1,0)+
              invK.at<double>(1,2)*piexpoint.at<double>(2,0);
     double c=invK.at<double>(2,0)*piexpoint.at<double>(0,0)+
              invK.at<double>(2,1)*piexpoint.at<double>(1,0)+
              invK.at<double>(2,2)*piexpoint.at<double>(2,0);
             
     raypoint=(Mat_<double>(3,1)<<a,b,c);
     rpoint.x=raypoint.at<double>(0,0);
     rpoint.y=raypoint.at<double>(1,0);
     rpoints.push_back(rpoint);
     }
 }
 
  void getpost(const Mat &R, const Mat &t, Mat &post)
 {
     hconcat(R,t,post);
 }
 
 void get3Dpoints(const Mat &R10, const Mat &t10, const  Mat &R21, const Mat &t21, 
                    const vector<Point2f> &p1, const vector<Point2f> &p2, 
                    const Mat &K,  vector<Point3f> &points)
 {
    /**
     Mat post10=(Mat_<double>(3,4)<<R10.at<double>(0,0), R10.at<double>(0,1), R10.at<double>(0,2),t10.at<double>(0,0),
                                    R10.at<double>(1,0), R10.at<double>(1,1), R10.at<double>(1,2),t10.at<double>(1,0),
                                    R10.at<double>(2,0), R10.at<double>(2,1), R10.at<double>(2,2),t10.at<double>(2,0));
     Mat post21=(Mat_<double>(3,4)<<R21.at<double>(0,0), R21.at<double>(0,1), R21.at<double>(0,2),t21.at<double>(0,0),
                                    R21.at<double>(1,0), R21.at<double>(1,1), R21.at<double>(1,2),t21.at<double>(1,0),
                                    R21.at<double>(2,0), R21.at<double>(2,1), R21.at<double>(2,2),t21.at<double>(2,0));
       **/                             
     
     Mat post10, post21;
     getpost(R10, t10, post10);
     getpost(R21, t21, post21);
     
     vector<Point2f> rpoint1, rpoint2;
     piex2rayplane(p1, K, rpoint1);
     piex2rayplane(p2, K, rpoint2);
     Mat ppoint;
     Point3f point3d;
     triangulatePoints(post10, post21, rpoint1, rpoint2,ppoint);
     for(int i=0;i<rpoint1.size();i++){
         point3d.x=ppoint.at<double>(i,0);
         point3d.y=ppoint.at<double>(i,1);
         point3d.z=ppoint.at<double>(i,2);
         points.push_back(point3d);
     }
         
 }
 
 
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

void bundleAdjustmentG2O(const VecVector3d &points_3d, const VecVector2d &points_2d,
                         const Mat &K, Sophus::SE3d &pose);

void bundleAdjustmentGaussNewton(const VecVector3d& points_3d, const VecVector2d& points_2d,
                                 const cv::Mat& K, Sophus::SE3d& pose)
{
    typedef Eigen::Matrix<double, 6, 1> Vector6d;
    const int  iterations=10;
    double cost=0, lastcost=0;
    double fx=K.at<double>(0,0);
    double fy=K.at<double>(1,1);
    double cx=K.at<double>(0,2);
    double cy=K.at<double>(1,2);
    
    
};



int main(int argc, char **argv) 
{
    Mat image1=imread("../1.jpg", IMREAD_ANYCOLOR);
    Mat image2=imread("../2.jpg", IMREAD_ANYCOLOR);
    assert(image1.data!=nullptr&&image2.data!=nullptr);
    
    double focallength, alph, imagewidth, imagehight;
    Mat K;
    focallength=1220.0;
    alph=1;
    imagewidth=1980;
    imagehight=1080;
    getcameraK(focallength, alph, imagewidth, imagehight,K);
    
    vector<KeyPoint> keypoints1, keypoints2;
    vector<DMatch> goodmatches;
    findmatch(image1, image2, keypoints1, keypoints2, goodmatches);
    cout<<goodmatches.size()<<endl;
    
    vector<Point2f> points1, points2;
    getmatchpoint(keypoints1, keypoints2, goodmatches, points1, points2);
    
    Mat Fun, Ess, R20, t20, R10, t10, R21, t21;
    gettransmat(points1, points2, K, Fun, Ess, R20, t20, R10, t10, R21, t21);
    
    vector<Point3f> points3d;
    get3Dpoints(R10,t10, R21, t21, points1, points2, K, points3d);
    
    
    
    

    
 
    return 0;
}
